#include "./mempool.h"
static VarArrayStruct* mempool = NULL;
static void* mempool_mem = NULL;
static unsigned int mempool_maxsize = 0;
static unsigned int mempool_current = 0;

void mempool_init(uint16_t size) {
  if (mempool == NULL) {
    mempool = varArray_new();
  }
  if (mempool_mem == NULL) {
    mempool_mem = malloc(size);
    if (mempool_mem != NULL) {
      mempool_maxsize = size;
      mempool_current = 0;
    } else {
      ERROR("mempool mem can not alloc.");
      // exit(error_memAlloc);
    }
  }
}
static void* alloc(uint16_t size) {
  void* p;
  if (size == 0)
    return NULL;
  if (mempool_current + size <= mempool_maxsize) {
    size_t current = mempool_current;
    mempool_current = mempool_current + size;
    p = (void*)((size_t)mempool_mem + current);
  } else {
    p = mem_alloc(size);
  }
  if (p == NULL) {
    ERROR("mempool can not alloc.");
    // exit(error_memAlloc);
  }
  return p;
}
static void mempool_addBlock(MempoolListStruct* mempoolList,
                             MempoolBlockStruct* mempoolBlock) {
  varArray_addData(mempoolList->used, mempoolBlock);
  mempoolBlock->list = mempoolList;
  mempoolBlock->unit = mempoolList->used;
}
void* mempool_alloc(uint16_t size) {
  MempoolListStruct* mempoolList;
  MempoolBlockStruct* mempoolBlock;

  if (size == 0)
    return NULL;
  for (unsigned int i = 0; i < mempool->length; i++) {
    mempoolList = (MempoolListStruct*)mempool->data[i];
    if (mempoolList->size == size) {
      if (mempoolList->free->length != 0) {
        for (int i = mempoolList->free->length - 1; i >= 0; i--) {
          mempoolBlock = (MempoolBlockStruct*)mempoolList->free->data[i];
          if (mempoolBlock->unit == mempoolList->free) {
            varArray_delIndex(mempoolList->free, i);
            mempool_addBlock(mempoolList, mempoolBlock);
            mem_set(&mempoolBlock->data, 0x00, size);
            return &mempoolBlock->data;
          } else {
            WARN("mempoolBlock be changed:%p.\n", mempoolBlock);
            varArray_delIndex(mempoolList->free, i);
          }
        }
      }
      mempoolBlock = (MempoolBlockStruct*)alloc(size + 2 * sizeof(void*));
      if (mempoolBlock == NULL) {
        return NULL;
      }
      mempool_addBlock(mempoolList, mempoolBlock);
      mem_set(&mempoolBlock->data, 0x00, size);
      return &mempoolBlock->data;
    }
  }
  mempoolList = (MempoolListStruct*)alloc(sizeof(MempoolListStruct));
  if (mempoolList == NULL) {
    return NULL;
  }
  varArray_addData(mempool, mempoolList);
  mempoolList->size = size;
  mempoolList->used = varArray_new();
  mempoolList->free = varArray_new();

  mempoolBlock = (MempoolBlockStruct*)alloc(size + 2 * sizeof(void*));
  if (mempoolBlock == NULL) {
    return NULL;
  }
  mempool_addBlock(mempoolList, mempoolBlock);
  mem_set(&mempoolBlock->data, 0x00, size);
  return &mempoolBlock->data;
}
void mempool_free(void* p) {
  MempoolListStruct* mempoolList;
  MempoolBlockStruct* mempoolBlock;
  if (p == NULL)
    return;

  mempoolBlock = (MempoolBlockStruct*)((size_t)p - 2 * sizeof(void*));
  mempoolList = mempoolBlock->list;
  if (mempoolBlock->unit == mempoolList->used) {
    unsigned int index = varArray_findData(mempoolList->used, mempoolBlock);
    if (index < mempoolList->used->length) {
      varArray_delIndex(mempoolList->used, index);
      varArray_addData(mempoolList->free, mempoolBlock);
      mempoolBlock->list = mempoolList;
      mempoolBlock->unit = mempoolList->free;
    }
  } else {
    WARN("can not free; mempoolBlock be changed:%p.\n", mempoolBlock);
  }
}
void mempool_info(void) {
  MempoolListStruct* mempoolList;

  printf("mempool start at %p; used %d in %d.\n", mempool_mem, mempool_current,
         mempool_maxsize);
  for (unsigned int i = 0; i < mempool->length; i++) {
    mempoolList = (MempoolListStruct*)mempool->data[i];
    printf("mem size:%d. used:%d; free:%d.\n", mempoolList->size,
           mempoolList->used->length, mempoolList->free->length);
  }
}
